#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include "robot.h"

int main(int argc, char **argv) {
	char c;
	char bumper;
	time_t start_time;
	time_t stamp;

	start_time = time(NULL);

	if(robot_init(argv[1]) < 0) {
		fprintf(stderr, "could not connect to the robot\n");
		exit(1);
	}

/*
	while(1) {
		//printf("> ");
		scanf("%c", &c);
		if(c == 'q') {
			robot_close();
			exit(0);
		}
		else if(c == 'f') {
			robot_forward();
		}
		else if(c == 'b') {
			robot_backward();
		}
		else if(c == 'l') {
			robot_left();
		}
		else if(c == 'r') {
			robot_right();
		}
		else if(c == 's') {
			robot_stop();
		}
		else if(c == 't') {
			robot_bumpers(&bumper, &stamp);
			printf("bumbers: ");
			if(bumper < 0 || bumper > 3) {
				printf("not valid data\n");
			}
			printf("left=%s   ", ((bumper & 2) == 2) ? "yes" : "no");
			printf("right=%s   ", ((bumper & 1) == 1) ? "yes" : "no");
			printf("time=%d\n", (int) difftime(stamp, start_time));
		}
	}
*/

	return 0;
}


void *watchKeyboard(void* param) {
	while(1) {
		//printf("> ");
		scanf("%c", &c);
		if(c == 'q') {
			robot_close();
			exit(0);
		}
		else if(c == 'f') {
			robot_forward();
		}
		else if(c == 'b') {
			robot_backward();
		}
		else if(c == 'l') {
			robot_left();
		}
		else if(c == 'r') {
			robot_right();
		}
		else if(c == 's') {
			robot_stop();
		}
		else if(c == 't') {
//			robot_bumpers(&bumper, &stamp);
//			printf("bumbers: ");
//			if(bumper < 0 || bumper > 3) {
//				printf("not valid data\n");
//			}
//			printf("left=%s   ", ((bumper & 2) == 2) ? "yes" : "no");
//			printf("right=%s   ", ((bumper & 1) == 1) ? "yes" : "no");
//			printf("time=%d\n", (int) difftime(stamp, start_time));
		}
	}
}


void *watchBumpers(void* param) {
}
